#include "physics.h"
#include <Box2D/Box2D.h>
#include "../gameobjects/graphicsobject.h"
#include "collisionshapes.h"

Physics::Physics(float worldScale, QPointF gravity)
    : m_fWorldScale(worldScale)
{
    b2Vec2 gVec(gravity.x(), gravity.y());
    m_pWorld = new b2World(gVec);
}

Physics::~Physics()
{
    delete m_pWorld;
    m_pWorld = 0;
}

bool Physics::createBody(QPointer<GraphicsObject> gobject)
{
    if(m_mapGOBody.contains(gobject)) return false;

    bool isDynamic;
    QPointF initialPos;
    float initialAngleDeg;
    bool isFixedRotation;
    float gravityScale;
    //gobject->physicsParameters(isDynamic, initialPos, initialAngleDeg,
    //                           isFixedRotation, gravityScale);

    // TODO: Convert position to physics space
    b2Body* pBody;// = createBody(isDynamic ? BT_Dynamic : BT_Static, initialPos.x(),
                  //             initialPos.y(), initialAngleDeg, isFixedRotation, gravityScale);

    if(gobject->hasAnimations())
    {

    }
    else
    {
        QList<QPointer<CollisionShape> > cshapes = gobject->collisionShapes();

        foreach(QPointer<CollisionShape> cshape, cshapes)
        {
            float friction, restitution, density;
            cshape->physicsParams(friction, restitution, density);

            if(QPointer<CollisionCircle> circle = qobject_cast<CollisionCircle*>(cshape))
            {
                b2CircleShape cs;
                // TODO: see if we have to convert!
                cs.m_p.x = circle->center().x();
                cs.m_p.y = circle->center().y();
                cs.m_radius = circle->radius();
                createFixture(pBody, &cs, friction, restitution, density);
            }
            else if(QPointer<CollisionBox> rectangle = qobject_cast<CollisionBox*>(cshape))
            {
                QRectF rect = rectangle->boxRect();
                b2Vec2 center;
                // TODO: see if we have to convert!
                center.x = rect.center().x();
                center.y = rect.center().y();
                b2PolygonShape bs;
                bs.SetAsBox(rect.width()/2, rect.height()/2, center, 0);
                createFixture(pBody, &bs, friction, restitution, density);
            }
            else if(QPointer<CollisionPoly> polygon = qobject_cast<CollisionPoly*>(cshape))
            {
                QPolygonF poly = polygon->poly();
                b2Vec2 vertices[8];
                int count = poly.size();
                b2PolygonShape ps;

                for(int i=0;i<count;++i)
                {
                    // TODO: see if we have to convert!
                    vertices[i].Set(poly[i].x(), poly[i].y());
                }

                ps.Set(vertices, count);
                createFixture(pBody, &ps, friction, restitution, density);
            }
        }
    }

    m_mapGOBody[gobject] = pBody;

    return true;
}

Physics::BasicBodyParameters Physics::basicBodyParameters(QPointer<GraphicsObject> gobject)
{
    BasicBodyParameters params;

    if(!m_mapGOBody.contains(gobject))
    {
        params.bValid = false;
        return params;
    }

    params.bValid = true;

    b2Body* pBody = m_mapGOBody[gobject];
    b2Vec2 pos = pBody->GetPosition();
    float angle = pBody->GetAngle();

    // TODO: Convert position in graphics space
    params.pos.setX(pos.x);
    params.pos.setY(pos.y);
    params.angleRad = angle;

    return params;
}

b2Body *Physics::createBody(Physics::BodyType type, float initialX, float initialY,
                            float initialAngleDeg, bool bFixedRotation, float gravityScale)
{
    b2BodyDef def;

    switch(type)
    {
    case BT_Static:
        def.type = b2_staticBody;
        break;
    case BT_Kinematic:
        def.type = b2_kinematicBody;
        break;
    case BT_Dynamic:
        def.type = b2_dynamicBody;
        break;
    }

    def.position = b2Vec2(initialX, initialY);
    def.angle = initialAngleDeg*3.1415f/180.0f;
    def.fixedRotation = bFixedRotation;
    def.gravityScale = gravityScale;

    return m_pWorld->CreateBody(&def);
}

b2Fixture *Physics::createFixture(b2Body *pBody, const b2Shape *pShape, float friction, float restitution, float density)
{
    b2FixtureDef def;
    def.shape = pShape;
    def.friction = friction;
    def.restitution = restitution;
    def.density = density;

    return pBody->CreateFixture(&def);
}

void Physics::updateWorld(float speed)
{
    float timeStep = speed*1.0f/60.0f;
    int velocityIterations = 10;
    int positionIterations = 6;
    m_pWorld->Step(timeStep, velocityIterations, positionIterations);

    // m_pWorld->ClearForces();
}

void Physics::setResolution(QPoint res)
{
    m_resolution = res;
}

float Physics::toGraphicsCoordinates(float value)
{
    return value*m_fWorldScale;
}

float Physics::toPhysicsCoordinates(float value)
{
    return value/m_fWorldScale;
}
